#include "RTUSlave.hpp"
#include "crc16.hpp"
#include <cstring>

using namespace Modbus;

RTUSlave::RTUSlave(PortableStrategy *portable_strategy,Configuration configuration) : Slave(portable_strategy) {
    this->portable_strategy = portable_strategy;
    this->configuration = configuration;
    this->rx_size = 0;
}

int RTUSlave::setAddress(unsigned int address){
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( this->portable_strategy->lock() != true ){
        return Result::LOCK_FAILED;
    }
    this->configuration.address = address;
    if ( this->portable_strategy->unlock() != true ){
        return Result::UNLOCK_FAILED;
    }
    return Result::SUCCESS;
}

int RTUSlave::getAddress(unsigned int *address_buffer){
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( this->portable_strategy->lock() != true ){
        return Result::LOCK_FAILED;
    }
    *address_buffer = this->configuration.address;
    if ( this->portable_strategy->unlock() != true ){
        return Result::UNLOCK_FAILED;
    }
    return Result::SUCCESS;
}

int RTUSlave::sendResponse(void *pdu,unsigned int size){
    int ret = Result::SUCCESS;
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( this->portable_strategy->lock() != true ){
        return Result::LOCK_FAILED;
    }
    if ( this->configuration.tx_buffer_size < 3 ){
        ret = Result::BUFFER_IS_TOO_SMALL;
    }else if ( size <= this->configuration.tx_buffer_size - 3 ){
        memcpy(this->configuration.tx_buffer + 1,pdu,size);
        /* Construct ADU. */
        this->configuration.tx_buffer[0] = static_cast<uint8_t>(this->configuration.address);
        CRC16Calculator crc16_calculator;
        crc16_calculator.start();
        crc16_calculator.feed(this->configuration.tx_buffer,size + 1);
        uint16_t crc16_calculate = crc16_calculator.get();
        this->configuration.tx_buffer[size + 1] = crc16_calculate & 0xff;
        this->configuration.tx_buffer[size + 2] = (crc16_calculate >> 8) & 0xff;
        /* Send ADU. */
        ret = this->portable_strategy->send(this->configuration.tx_buffer,size + 3);
    }else{
        ret = Result::PDU_SIZE_OVER_BUFFER_SIZE;
    }
    if ( this->portable_strategy->unlock() != true ){
        return Result::UNLOCK_FAILED;
    }
    return ret;
}

int RTUSlave::feed(uint8_t byte){
    int ret = Result::SUCCESS;
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( this->portable_strategy->lock() != true ){
        return Result::LOCK_FAILED;
    }

    this->configuration.rx_buffer[this->rx_size] = byte;
    this->rx_size += 1;
    if ( this->rx_size >= this->configuration.rx_buffer_size ){
        ret = this->cut();
    }
    if ( this->portable_strategy->unlock() != true ){
        return Result::UNLOCK_FAILED;
    }
    return ret;
}

int RTUSlave::cut(void){
    int ret = Result::SUCCESS;
    if ( this->portable_strategy == nullptr ){
        return Result::NO_PORTABLE_STRATEGY;
    }
    if ( this->portable_strategy->lock() != true ){
        return Result::LOCK_FAILED;
    }
    if ( this->rx_size >= 4 ){
        uint16_t crc16_frame = 0x0000;
        uint16_t crc16_calculate = 0x0000;
        bool flag_broadcast = false;
        if ( this->configuration.rx_buffer[0] == static_cast<uint8_t>(this->configuration.address) || this->configuration.rx_buffer[0] == 0 ){
            if ( this->configuration.rx_buffer[0] == 0 ){
                flag_broadcast = true;
            }
            /* Check CRC-16. */
            CRC16Calculator crc16_calculator;
            crc16_calculator.start();
            crc16_calculator.feed(this->configuration.rx_buffer,this->rx_size - 2);
            crc16_calculate = crc16_calculator.get();
            crc16_frame |= (this->configuration.rx_buffer[this->rx_size - 1]) << 8;
            crc16_frame |= (this->configuration.rx_buffer[this->rx_size - 2]);
            if ( crc16_frame == crc16_calculate ){
                /* Check pass. find function. */
                int n_function = 0;
                int res = Result::NO_FUNCTION;
                for ( n_function = 0; n_function < Slave::MAX_FUNCTION_COUNT; n_function++ ){
                    if ( this->function_list[n_function] == nullptr ){
                        continue;
                    }
                    if ( static_cast<uint8_t>(this->function_list[n_function]->getFunctionCode()) == this->configuration.rx_buffer[1] ){
                        res = this->function_list[n_function]->parse(this->configuration.rx_buffer + 1,this->rx_size - 3,flag_broadcast);
                        break;
                    }
                }
                /* Process result. */
                if ( res != Result::SUCCESS && flag_broadcast == false ){
                    if ( this->configuration.tx_buffer_size >= 5 ){
                        this->configuration.tx_buffer[0] = static_cast<uint8_t>(this->configuration.address);
                        this->configuration.tx_buffer[1] = 0x80 | this->configuration.rx_buffer[1];
                        if ( res == Result::NO_FUNCTION ){
                            /* Function code not support. */
                            this->configuration.tx_buffer[2] = 0x01;
                        }else if ( res == Result::INVALID_DATA_ADDRESS ){
                            /* Invalid data address. */
                            this->configuration.tx_buffer[2] = 0x02;
                        }else if ( res == Result::INVALID_DATA_VALUE ){
                            /* Invalid data value. */
                            this->configuration.tx_buffer[2] = 0x03;
                        }else if ( res == Result::DEVICE_FAULT ){
                            /* Device fault. */
                            this->configuration.tx_buffer[2] = 0x04;
                        }else if ( res == Result::DEVICE_BUSY ){
                            /* Device busy. */
                            this->configuration.tx_buffer[2] = 0x07;
                        }else{
                            this->configuration.tx_buffer[2] = 0x04;
                        }
                        crc16_calculator.start();
                        crc16_calculator.feed(this->configuration.tx_buffer,3);
                        crc16_calculate = crc16_calculator.get();
                        this->configuration.tx_buffer[3] = (crc16_calculate & 0xff);
                        this->configuration.tx_buffer[4] = (crc16_calculate >> 8) & 0xff;
                        ret = this->portable_strategy->send(this->configuration.tx_buffer,5);
                    }
                }
            }
        }
    }
    this->rx_size = 0;
    if ( this->portable_strategy->unlock() != true ){
        return Result::UNLOCK_FAILED;
    }
    return ret;
}